package de.upb.aquarium.api.objects;

import de.upb.aquarium.api.utility.VectorFloat;
import java.lang.Math;

public class PathCalculator implements IPathCalculator{

	private VectorFloat[] wps;
	private float speed;
	private VectorFloat currentStart;
	private VectorFloat currentEnd;
	private int posCounter = 0;
	private VectorFloat[] splitPoints;
	private int wpCounter = 1;
	private float oldRotation = 0;
	private float newRotation = 0;
	private int smoothRotationStep = 0;
	private float smoothAngle = 0;
	
	public PathCalculator(VectorFloat[] waypoints, float speedfactor){
		this.wps = waypoints;
		this.speed = speedfactor;
	}
	
	private void init(){
		if(wps.length > 1){
			currentStart = wps[0];
			currentEnd = wps[1];
		}
	}
	
	private VectorFloat[] calculatePositions(){
		VectorFloat[] points;
		int steps = 0;
		if(speed < 1){
			steps = (int)Math.abs(1/speed);
		}
		if(steps <= 1){
			points = new VectorFloat[1];
			points[0] = currentEnd;
			return points;
		}else{
			points = new VectorFloat[steps];
			VectorFloat currentPos = currentStart;
			VectorFloat delta = currentEnd.sub(currentStart);
			for(int i=0; i < 3; i++){
				delta.data[i] = delta.data[i] / steps;
			}
			for(int i=0; i < steps; i++){
				currentPos = currentPos.add(delta);
				points[i] = currentPos;
			}
			return points;
		}
	}
	
	private void setNextPoints(){
		if(wps.length - 1 > wpCounter){
			currentStart = wps[wpCounter];
			currentEnd = wps[wpCounter + 1];
			wpCounter++;
		}else if(wps.length - 1 == wpCounter){
			currentStart = wps[wpCounter];
			currentEnd = wps[0];
			wpCounter++;
		}else{
			wpCounter = 1;
			currentStart = null;
		}
	}
	
	public float calculateRotation(){
		if(currentStart != null){
			oldRotation = newRotation;
			VectorFloat dir = currentEnd.sub(currentStart);
			dir.normalize();
			float angle;
			if(dir.data[2] >= 0){
				angle = (float)Math.asin(dir.data[0]);
				angle = angle * 180 / (float)Math.PI;
				angle = (angle + 90);
			}else{
				angle = (float)Math.asin(dir.data[0]);
				angle = angle * 180 / (float)Math.PI;
				angle = (angle + 90) * -1;
			}
			newRotation = angle;
			if(speed <= 0.01){
				if(oldRotation != newRotation){
					smoothRotationStep = 0;
					smoothAngle = (newRotation - oldRotation) / 20;
					return oldRotation + smoothAngle;
				}else if(smoothRotationStep < 19){
					angle = newRotation;
					for(int i=smoothRotationStep; i <= 18; i++){
						angle = angle - smoothAngle;
					}
					smoothRotationStep++;
					return angle;
				}else{
					return angle;
				}
			}else{
				return angle;
			}
		}
		return 0f;
	}
	
	@Override
	public float[] nextPosition() {
		if(currentStart == null){
			init();
			return currentStart.data;
		}else if(posCounter == 0){
			splitPoints = calculatePositions();
			posCounter++;
			if(splitPoints.length <= posCounter){
				posCounter = 0;
				setNextPoints();
			}
			return splitPoints[0].data;
		}else{
			if(splitPoints.length - 1 > posCounter){
				posCounter++;
				return splitPoints[posCounter].data;
			}else{
				int i = posCounter;
				posCounter = 0;
				setNextPoints();
				return splitPoints[i].data;
			}
		}
	}

}
